/*
 * Copyright (c) 2019, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2019-08-26     sogwms       The first version
 */

#include "ps2.h"
#include "ps2_table.h"

#define THREAD_DELAY_TIME           30

#define THREAD_PRIORITY             ((256 / 3) + 3)
#define THREAD_STACK_SIZE           1024
#define THREAD_TIMESLICE            10

#define KEEP_TIME()                 _delay_us(16);

static uint8_t light_mode = PS2_NO_MODE;
static aos_task_t tid_ps2 = NULL;
static struct ps2_table table[PS2_TABLE_SIZE] = PS2_DEFAULT_TABLE;

static rvm_dev_t *ps2_cs_pin = NULL;
static rvm_dev_t *ps2_clk_pin = NULL;
static rvm_dev_t *ps2_do_pin = NULL;
static rvm_dev_t *ps2_di_pin = NULL;

static void hal_cs_high(void)
{
    rvm_hal_gpio_pin_write(ps2_cs_pin, RVM_GPIO_PIN_HIGH);
}
static void hal_cs_low(void)
{
    rvm_hal_gpio_pin_write(ps2_cs_pin, RVM_GPIO_PIN_LOW);
}
static void hal_clk_high(void)
{
    rvm_hal_gpio_pin_write(ps2_clk_pin, RVM_GPIO_PIN_HIGH);
}
static void hal_clk_low(void)
{
    rvm_hal_gpio_pin_write(ps2_clk_pin, RVM_GPIO_PIN_LOW);
}
static void hal_do_high(void)
{
    rvm_hal_gpio_pin_write(ps2_do_pin, RVM_GPIO_PIN_HIGH);
}
static void hal_do_low(void)
{
    rvm_hal_gpio_pin_write(ps2_do_pin, RVM_GPIO_PIN_LOW);
}
static int hal_read_di(void)
{ 
    rvm_hal_gpio_pin_data_t val;
    rvm_hal_gpio_pin_read(ps2_di_pin, &val);
    return val;
}

static void _delay_us(uint16_t us)
{
    for (int i = 0; i < us; i++)
    {
        for (int j = 0; j < 0x1F;)
            j++;
    }
}

static uint8_t _transfer(uint8_t data)
{
    uint8_t temp = 0;

    for (uint16_t i = 0x01; i < 0x0100; i <<= 1)
    {
        if (i & data)
            hal_do_high(); 
        else
            hal_do_low();

        hal_clk_high();
        KEEP_TIME();
        hal_clk_low();
        if (hal_read_di())
            temp = i | temp;
        KEEP_TIME();
        hal_clk_high();
    }
    
    return temp;
}

static void transfer(const uint8_t *pb_send, uint8_t *pb_recv, uint8_t len)
{
    hal_cs_low();
    _delay_us(16);
    for (uint8_t i = 0; i < len; i++)
    {
        pb_recv[i] = _transfer(pb_send[i]);
    }
    hal_cs_high();
    _delay_us(16);
}

int ps2_scan(ps2_ctrl_data_t pt)
{
    uint8_t temp[9] = {0};

    temp[0] = 0x01;
    temp[1] = 0x42;
    temp[3] = 0;
    temp[4] = 0;

    transfer(temp, temp, 9);

    pt->button = temp[3] | (temp[4] << 8);
    pt->right_stick_x = temp[5];
    pt->right_stick_y = temp[6];
    pt->left_stick_x = temp[7];
    pt->left_stick_y = temp[8];

    if (temp[2] == 0x5A)
    {
        light_mode = temp[1];
        return 1;
    }
    else
    {
        light_mode = PS2_NO_MODE;
    }

    return 0;
}

/**
 * @return PS2_GREEN_MDOE or PS2_RED_MDOE or other(no connect) 
 */
int ps2_read_light(void)
{
    return light_mode;
}

static signed long ps2_sender_send(unsigned short cmd, void *param, unsigned short size)
{
    // TODO

    if (cmd == COMMAND_RC_VIBRATE)
    {

    }
    else
    {
        return 1;
    }

    return 0;
}

static struct command_sender ps2_sender = {
    .name = "ano",
    .send = ps2_sender_send
};

command_sender_t ps2_get_sender(void)
{
    return &ps2_sender;
}

static void ps2_thread_entry(void *param)
{
    struct ps2_ctrl_data ctrl_data;
    struct cmd_velocity target_velocity;

    while (1)
    {
        aos_msleep(THREAD_DELAY_TIME);   
        ps2_scan(&ctrl_data);
        // look-up table and send standard command
        for (int i = 0; i < PS2_TABLE_SIZE; i++)
        {
            if (!(ctrl_data.button & table[i].ps2_cmd))
            {
                if (table[i].standard_cmd != COMMAND_NONE)
                {
                    command_handle(table[i].standard_cmd, NULL, 0);
                }
            }
        }

        // rocker
        if (ps2_read_light() == PS2_RED_MODE)
        {
            uint8_t value[4] = {
                ctrl_data.left_stick_x, 
                ctrl_data.right_stick_x,
                ctrl_data.left_stick_y,
                ctrl_data.right_stick_y};

            signed short cmd[4] = {
                table[PS2_ROCKER_LX].standard_cmd,
                table[PS2_ROCKER_LY].standard_cmd,
                table[PS2_ROCKER_RX].standard_cmd,
                table[PS2_ROCKER_RY].standard_cmd};

            for (int i = 0; i < 4; i++)
            {
                if (cmd[i] != COMMAND_NONE)
                {
                    float tmp = CHASSIS_VELOCITY_LINEAR_MAXIMUM;
                    if (cmd[i] == COMMAND_SET_CHASSIS_VELOCITY_ANGULAR_Z)
                    {
                        tmp = CHASSIS_VELOCITY_ANGULAR_MAXIMUM;
                    }
                    
                    target_velocity.data.common = tmp * ((0x80 - (int)(i/2)) - value[i]) / 128;
                    command_handle(cmd[i], &target_velocity, sizeof(struct cmd_velocity));
                }
            }
        }
    }
}

void ps2_init(signed long cs_pin, signed long clk_pin, signed long do_pin, signed long di_pin)
{
    char filename[16];
    rvm_gpio_pin_drv_register(cs_pin);
    rvm_gpio_pin_drv_register(clk_pin);
    rvm_gpio_pin_drv_register(do_pin);
    rvm_gpio_pin_drv_register(di_pin);
  
    sprintf(filename, "gpio_pin%ld", cs_pin);
    ps2_cs_pin = rvm_hal_gpio_pin_open(filename);

    sprintf(filename, "gpio_pin%ld", clk_pin);
    ps2_clk_pin = rvm_hal_gpio_pin_open(filename);

    sprintf(filename, "gpio_pin%ld", do_pin);
    ps2_do_pin = rvm_hal_gpio_pin_open(filename);

    sprintf(filename, "gpio_pin%ld", do_pin);
    ps2_di_pin = rvm_hal_gpio_pin_open(filename);


    rvm_hal_gpio_pin_set_direction(ps2_cs_pin,  RVM_GPIO_DIRECTION_OUTPUT);
    rvm_hal_gpio_pin_set_direction(ps2_clk_pin, RVM_GPIO_DIRECTION_OUTPUT);
    rvm_hal_gpio_pin_set_direction(ps2_do_pin,  RVM_GPIO_DIRECTION_OUTPUT);
    rvm_hal_gpio_pin_set_direction(ps2_di_pin,  RVM_GPIO_DIRECTION_INPUT);

    hal_cs_high();
    hal_clk_high();

    aos_task_create(&tid_ps2,
                    "ano",
                    ps2_thread_entry, NULL,
                    NULL,
                    THREAD_STACK_SIZE,
                    THREAD_PRIORITY,
                    AOS_TASK_AUTORUN);

    if (tid_ps2 == NULL)
    {
        aos_cli_printf("Can't create thread for ps2");
    }
}
